from time import sleep
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
steer_pwms=[]
angels=[90,90,120,70,70,90]


# Steer related functions
def setServoAngle(servoID, angle):
    if servoID<0 or servoID>=len(steer_pwms):
        print("Alert:servoID error: "+str(servoID))
        return
    steer_pwms[servoID].ChangeDutyCycle(angle / 18. + 3.)
    sleep(0.3)
    if servoID != -1:
        steer_pwms[servoID].stop()
    print("Steer "+str(servoID)+" has been turned to "+str(angle))

def restoreServos():
    for i in range(len(angels)):
        setServoAngle(i,angels[i])
        # sleep(2)



# Init steering engines
steers = [7,11,13,33,15,35]
for i in range(len(steers)):
    GPIO.setup(steers[i],GPIO.OUT)
    pwm=GPIO.PWM(steers[i],50)
    pwm.start(angels[i] / 18. + 3.)
    # sleep(0.3)
    steer_pwms.append(pwm)
restoreServos()


# Running
print("Keeping...")
sleep(5)


# Exit
steer_pwms[2].stop()
